
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sitl_copter.c
  * @author     baiyang
  * @date       2021-7-26
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sitl_copter.h"
#include <SitlMultiCopter/Multicopter.h>

#include <stdint.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>

#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
#include <common/console/console.h>
#include <common/geo/declination.h>
#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/
#define SITLCOPTER_TASK_THREAD_PRIORITY     5
#define EVENT_SITLCOPTER_UPDATE             (1 << 0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static struct rt_timer sitlcopter_timer;
static struct rt_event sitlcopter_event;

static char thread_sitlcopter_stack[1024*10];
struct rt_thread thread_sitlcopter_handle;

uitc_vehicle_home vehicle_home = {0};
uitc_vehicle_origin vehicle_origin = {0};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void sitlcopter_timer_update(void* parameter)
{
    rt_event_send(&sitlcopter_event, EVENT_SITLCOPTER_UPDATE);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sitlcopter_state_decode()
{
    uint8_t res = false;
    DEFINE_TIMETAG(baro_update, 20);

    uint64_t now_us = time_micros64();

    uitc_vehicle_hil_state hil_state;
    if (itc_copy_from_hub(ITC_ID(vehicle_hil_state), &hil_state) != 0) {
        return;
    }

    if (!vehicle_home.valid_alt) {
        vehicle_home.timestamp_us = now_us;
        vehicle_home.alt = hil_state.alt * 1e-3;
        vehicle_home.z   = hil_state.alt * 1e-3;
        vehicle_home.valid_alt = true;
        itc_publish(ITC_ID(vehicle_home), &vehicle_home);

        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.alt = hil_state.alt * 1e-3;
        vehicle_origin.z   = hil_state.alt * 1e-3;
        vehicle_origin.valid_alt = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (!vehicle_home.valid_hpos) {
        vehicle_home.timestamp_us = now_us;
        vehicle_home.lat = hil_state.lat;
        vehicle_home.lon = hil_state.lon;

        vehicle_home.x = 0.0f;
        vehicle_home.y = 0.0f;

        vehicle_home.yaw = hil_state.yaw;
        vehicle_home.mag_decl = geo_get_declination((double)vehicle_home.lat*1e-7, (double)vehicle_home.lon*1e-7);
        vehicle_home.valid_hpos = true;

        itc_publish(ITC_ID(vehicle_home), &vehicle_home);

        vehicle_origin.timestamp_us = now_us;
        vehicle_origin.lat = hil_state.lat;
        vehicle_origin.lon = hil_state.lon;

        vehicle_origin.x = 0.0f;
        vehicle_origin.y = 0.0f;

        vehicle_origin.yaw = hil_state.yaw;
        vehicle_origin.mag_decl = geo_get_declination((double)vehicle_home.lat*1e-7, (double)vehicle_home.lon*1e-7);
        vehicle_origin.valid_hpos = true;

        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (!vehicle_home.valid_lpos && vehicle_home.valid_hpos && vehicle_home.valid_alt) {
        vehicle_home.valid_lpos = true;
        itc_publish(ITC_ID(vehicle_home), &vehicle_home);
    }

    if (!vehicle_origin.valid_lpos && vehicle_origin.valid_hpos && vehicle_origin.valid_alt) {
        vehicle_origin.valid_lpos = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    uitc_vehicle_attitude vehicle_attitude;
    vehicle_attitude.timestamp_us = now_us;
    vehicle_attitude.vehicle_euler.roll = hil_state.roll;
    vehicle_attitude.vehicle_euler.pitch = hil_state.pitch;
    vehicle_attitude.vehicle_euler.yaw = hil_state.yaw;

    quat_from_euler(&vehicle_attitude.vehicle_quat, vehicle_attitude.vehicle_euler.roll,
                    vehicle_attitude.vehicle_euler.pitch, vehicle_attitude.vehicle_euler.yaw);
    quat_norm(&vehicle_attitude.vehicle_quat);
    
    itc_publish(ITC_ID(vehicle_attitude), &vehicle_attitude);

    uitc_sensor_gyr sensor_gyr;
    sensor_gyr.timestamp_us = now_us;
    sensor_gyr.sensor_gyr_measure[0] = hil_state.rollspeed;
    sensor_gyr.sensor_gyr_measure[1] = hil_state.pitchspeed;
    sensor_gyr.sensor_gyr_measure[2] = hil_state.yawspeed;

    sensor_gyr.sensor_gyr_correct[0] = hil_state.rollspeed;
    sensor_gyr.sensor_gyr_correct[1] = hil_state.pitchspeed;
    sensor_gyr.sensor_gyr_correct[2] = hil_state.yawspeed;

    sensor_gyr.sensor_gyr_filter[0] = hil_state.rollspeed;
    sensor_gyr.sensor_gyr_filter[1] = hil_state.pitchspeed;
    sensor_gyr.sensor_gyr_filter[2] = hil_state.yawspeed;

    itc_publish(ITC_ID(sensor_gyr), &sensor_gyr);

    Vector3f_t acc_n = { 0 };
    Vector3f_t acc_b = { 0 };
    Vector3f_t acc_bf = { 0 };

    acc_bf.x = (float)hil_state.xacc * 0.001f * GRAVITY_MSS;
    acc_bf.y = (float)hil_state.yacc * 0.001f * GRAVITY_MSS;
    acc_bf.z = (float)hil_state.zacc * 0.001f * GRAVITY_MSS;

    quat_vec_rotate(&vehicle_attitude.vehicle_quat, &acc_n, &acc_bf);

    // 模拟加速度计数据，添加重力加速度
    acc_n.z -= GRAVITY_MSS;
    quat_inv_vec_rotate(&vehicle_attitude.vehicle_quat, &acc_b, &acc_n);

    uitc_sensor_acc sensor_acc;
    sensor_acc.timestamp_us = now_us;
    sensor_acc.sensor_acc_measure[0] = acc_b.vec3[0];
    sensor_acc.sensor_acc_measure[1] = acc_b.vec3[1];
    sensor_acc.sensor_acc_measure[2] = acc_b.vec3[2];

    sensor_acc.sensor_acc_correct[0] = acc_b.vec3[0];
    sensor_acc.sensor_acc_correct[1] = acc_b.vec3[1];
    sensor_acc.sensor_acc_correct[2] = acc_b.vec3[2];

    sensor_acc.sensor_acc_filter[0] = acc_b.vec3[0];
    sensor_acc.sensor_acc_filter[1] = acc_b.vec3[1];
    sensor_acc.sensor_acc_filter[2] = acc_b.vec3[2];
    
    itc_publish(ITC_ID(sensor_acc), &sensor_acc);

    Euler_t e_tmp;
    Quat_t att_q_tmp = { 0 };
    e_tmp.pitch = 0.0f;
    e_tmp.roll = 0.0f;
    e_tmp.yaw = Deg2Rad(vehicle_origin.mag_decl);

    quat_from_euler(&att_q_tmp, e_tmp.roll, e_tmp.pitch, e_tmp.yaw);
    quat_norm(&att_q_tmp);

    Vector3f_t mag = { 0 };
    Vector3f_t mag_vec = { 0 };
    Vector3f_t mag_vec_tmp = { 0 };
    uitc_sensor_mag sensor_mag;
    if (vehicle_origin.valid_lpos) {
        mag.x = 0.500f;
        mag.y = 0.0f;
        mag.z = 0.0f;
        quat_vec_rotate(&att_q_tmp, &mag_vec_tmp, &mag);
        quat_inv_vec_rotate(&vehicle_attitude.vehicle_quat, &mag_vec, &mag_vec_tmp);
        sensor_mag.timestamp_us = now_us;

        sensor_mag.sensor_mag_measure[0] = mag_vec.x;
        sensor_mag.sensor_mag_measure[1] = mag_vec.y;
        sensor_mag.sensor_mag_measure[2] = mag_vec.z;

        sensor_mag.sensor_mag_correct[0] = mag_vec.x;
        sensor_mag.sensor_mag_correct[1] = mag_vec.y;
        sensor_mag.sensor_mag_correct[2] = mag_vec.z;

        sensor_mag.sensor_mag_filter[0] = mag_vec.x;
        sensor_mag.sensor_mag_filter[1] = mag_vec.y;
        sensor_mag.sensor_mag_filter[2] = mag_vec.z;
        
        itc_publish(ITC_ID(sensor_mag), &sensor_mag);
    }

    uitc_vehicle_alt alt_info;
    alt_info.timestamp_us = now_us;
    alt_info.alt = (float)hil_state.alt * 0.001f;
    alt_info.relative_alt = (float)hil_state.alt * 0.001f;
    alt_info.vz = -(float)hil_state.vz * 0.01f;
    alt_info.az = -(acc_n.z + GRAVITY_MSS);
    alt_info.az_bias = 0;
    itc_publish(ITC_ID(vehicle_alt), &alt_info);

    if (time_check_tag(TIMETAG(baro_update))) {
        uitc_sensor_baro sensor_baro = { 0 };
        sensor_baro.timestamp_us = now_us;
        sensor_baro.altitude_cm = alt_info.relative_alt*100; // U coordinate
        sensor_baro.velocity_cms = alt_info.vz*100;

        /* publish baro data */
        itc_publish(ITC_ID(sensor_baro), &sensor_baro);
    }

    float fSinLat;
    float fCosLat;
    float Rmh;        //补偿半径
    float Rnh;
    float ff = 0.0033528132f;
    fSinLat = sinf(Deg2Rad((double)vehicle_origin.lat*1e-7));
    fCosLat = cosf(Deg2Rad((double)vehicle_origin.lat*1e-7));

    Rmh = (6378137 * (1.0f - 2.0f * ff + 3.0f * ff * fSinLat * fSinLat) + alt_info.alt);    //经高度和维度补偿后的半径
    Rnh = (6378137 * (1.0f + ff * fSinLat * fSinLat) + alt_info.alt);

    uitc_vehicle_position pos_info;
    pos_info.timestamp_us = now_us;
    pos_info.lat = hil_state.lat;
    pos_info.lon = hil_state.lon;
    pos_info.x = Deg2Rad(((double)hil_state.lat*1e-7) - (double)vehicle_origin.lat*1e-7) * Rmh;
    pos_info.y = Deg2Rad(((double)hil_state.lon*1e-7) - (double)vehicle_origin.lon*1e-7) * Rnh * fCosLat;
    pos_info.vx = (float)hil_state.vx * 0.01f;
    pos_info.vy = (float)hil_state.vy * 0.01f;
    pos_info.ax = acc_n.x;
    pos_info.ay = acc_n.y;
    pos_info.ax_bias = 0;    // TODO, no estimation bias for ax and ay
    pos_info.ay_bias = 0;
    itc_publish(ITC_ID(vehicle_position), &pos_info);

    TIMETAG_CHECK_EXECUTE(sensor_gps,100,{
        uitc_sensor_gps gps_pos_t;
        gps_pos_t.timestamp_us = now_us;
        gps_pos_t.lat = hil_state.lat;
        gps_pos_t.lon = hil_state.lon;
        gps_pos_t.alt = hil_state.alt;
        gps_pos_t.hdop = 1.5f;
        gps_pos_t.vdop = 2.0f;
        gps_pos_t.horizontal_accuracy = 1.5f;
        gps_pos_t.vertical_accuracy = 2.0f;
        gps_pos_t.vel_m_s = sqrtf(pos_info.vx * pos_info.vx
                  + pos_info.vy * pos_info.vy + alt_info.vz * alt_info.vz);
        gps_pos_t.vel_n_m_s = pos_info.vx;
        gps_pos_t.vel_e_m_s = pos_info.vy;
        gps_pos_t.vel_d_m_s = -alt_info.vz;
        gps_pos_t.vel_ned_valid = 1;
        gps_pos_t.fix_type = 3;
        gps_pos_t.status = GPS_OK_FIX_3D;
        gps_pos_t.num_sats = 18;
        itc_publish(ITC_ID(sensor_gps), &gps_pos_t);}
    )
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sitlcopter_entry(void* parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = EVENT_SITLCOPTER_UPDATE;

    while (1) {
        /* wait event occur */
        res = rt_event_recv(&sitlcopter_event, wait_set,
            RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
            RT_WAITING_FOREVER, &recv_set);

        uint32_t now_ms = time_millis();
        static uint32_t last_ms = 0;

        if (last_ms == 0) {
            last_ms = now_ms;
        }

#if 0  // 20210728 早期的测试程序，windows下，此代码的更新频率为1/0.067
        uitc_actuator_controls sim_in;
        sim_in.timestamp_us = time_micros64();
        sim_in.control[0] = 0.58f;
        sim_in.control[1] = 0.58f;
        sim_in.control[2] = 0.58f;
        sim_in.control[3] = 0.58f;
        itc_publish(ITC_ID(vehicle_actuator_controls), &sim_in);
#endif

        if (res == RT_EOK) {
            if (recv_set & EVENT_SITLCOPTER_UPDATE) {
                TIMETAG_CHECK_EXECUTE2(sitl_copter, 5, now_ms, Multi_RunMain((now_ms-last_ms)*1e-3);last_ms = now_ms;)
                TIMETAG_CHECK_EXECUTE2(state_decode, 5, now_ms, sitlcopter_state_decode();)
                TIMETAG_CHECK_EXECUTE2(sim_send_data, 30, now_ms, sim_flightgear_send_data();)
            }
        } else {
            // some err happen
            console_printf("sitl copter loop, err:%d\r\n", res);
        }
    }
    res = 0;

}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sitlcopter_init()
{
    rt_err_t res;

    /* create event */
    rt_event_init(&sitlcopter_event, "sitl", RT_IPC_FLAG_FIFO);

    /* register timer event */
    rt_timer_init(&sitlcopter_timer, "sitl", sitlcopter_timer_update, RT_NULL, 1,
        RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&sitlcopter_timer);

    res = rt_thread_init(&thread_sitlcopter_handle,
        "sitl",
        sitlcopter_entry,
        RT_NULL,
        &thread_sitlcopter_stack[0],
        sizeof(thread_sitlcopter_stack), SITLCOPTER_TASK_THREAD_PRIORITY, 1);
    RT_ASSERT(res == RT_EOK);
    rt_thread_startup(&thread_sitlcopter_handle);
}
/*------------------------------------test------------------------------------*/


